#include "global.h"
#include "modbus.h"
#include "device.h"

uchar Read_RW_bit(ushort addr, uchar n)
{
    uchar v;
    uchar m = addr & 7; // addr % 8
    addr >>= 3;         // addr / 8

    v = RW_BIT_FIRST[addr] >> m;

    if (n + m > 8)
        v |= RW_BIT_FIRST[addr+1] << (8 - m);

    return v & ((1 << n) - 1);
}

uchar Read_RO_bit(ushort addr, uchar n)
{
    uchar v;
    uchar m = addr & 7; // addr % 8
    addr >>= 3;         // addr / 8

    v = RO_BIT_FIRST[addr] >> m;

    if (n + m > 8)
        v |= RO_BIT_FIRST[addr+1] << (8 - m);

    return v & ((1 << n) - 1);
}

void Write_bit(ushort addr, uchar val, uchar n)
{
    uchar mask = (1 << n) - 1;
    uchar m = addr & 7; // addr % 8
    addr >>= 3;         // addr / 8

    // 将不使用的位清零
    val &= mask;

    RW_BIT_FIRST[addr] &= ~(mask << m);
    RW_BIT_FIRST[addr] |= val << m;

    if (n + m > 8)
    {
        RW_BIT_FIRST[addr+1] &= ~(mask >> (8 - m));
        RW_BIT_FIRST[addr+1] |= val  >> (8 - m);
    }
    if(addr == 0)
    {
        IO_MAGENT = MAGENT_OUTPUT(val);

    // 为兼容旧版本，接收到打开电磁铁指令时合并打开抓手电机
#if (PCB_VERSION == PCB_A2)
        pending_action = 1;
        action = XY_ACTION_CLOSE_PICKER + (val & 0x01);
#endif
    }
}
